#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
import geometry_msgs.msg
from copy import deepcopy
import numpy as np
# import keyboard
import termios
import tty
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from mqtt_integration.msg import TrajectoryInfo ,TimeInfo

def getKey():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        key = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return key

class TrajectoryPublisher:
    def __init__(self):
        self.pub = rospy.Publisher('trajectory_info', TrajectoryInfo, queue_size=10)
        self.current_trajectory = 0
        self.total_executions = 0
        rospy.Timer(rospy.Duration(2), self.publish_info_timer)  # 每秒调用一次publish_info_timer

    def publish_info(self):
        info = TrajectoryInfo()
        info.current_trajectory = self.current_trajectory
        info.total_executions = self.total_executions
        # rospy.loginfo(f"Publishing: {info}")
        self.pub.publish(info)

    def publish_info_timer(self, event):
        self.publish_info()


class MoveItCartesianDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)
        # 通过self引用time_info_callback
        rospy.Subscriber('time_info', TimeInfo, self.time_info_callback)  # 添加订阅时间

        # 初始化TrajectoryPublisher
        self.trajectory_publisher = TrajectoryPublisher()

        # 用于存储时间信息的变量
        self.current_time_hex = None
        self.elapsed_time_str = None

        # 是否需要使用笛卡尔空间的运动规划
        self.cartesian = rospy.get_param('~cartesian', True)

        # 初始化MoveGroupCommander
        self.arm = MoveGroupCommander('manipulator')

        # 当运动规划失败后，允许重新规划
        self.arm.allow_replanning(True)
       
        # 设置目标位置所使用的参考坐标系
        self.arm.set_pose_reference_frame('base_link')
               
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        self.arm.set_goal_position_tolerance(0.0001)
        self.arm.set_goal_orientation_tolerance(0.0001)
       
        # 设置允许的最大速度和加速度
        self.arm.set_max_acceleration_scaling_factor(0.1)
        self.arm.set_max_velocity_scaling_factor(0.1)
       
        # 获取终端link的名称
        end_effector_link = self.arm.get_end_effector_link()

        # 控制机械臂先回到初始化位置
        self.arm.set_named_target('home')
        self.arm.go()
        rospy.sleep(1)
                                               
        # 获取当前位姿数据为机械臂运动的起始位姿
        self.start_pose = self.arm.get_current_pose(end_effector_link).pose

    def time_info_callback(self, msg):
        # 保存接收到的time_info消息
        self.current_time_hex = msg.current_time
        self.elapsed_time_str = msg.elapsed_time
        # rospy.loginfo(f"Received time info: {msg.current_time}, elapsed: {msg.elapsed_time}")

    def get_time_info(self):
        # 提供获取时间信息的方法
        return self.current_time_hex, self.elapsed_time_str


    def execute_trajectory(self, trajectory_method, trajectory_number):
        self.trajectory_publisher.total_executions += 1
        self.trajectory_publisher.current_trajectory = trajectory_number
        # self.trajectory_publisher.publish_info()
        trajectory_method()
        self.trajectory_publisher.current_trajectory = 0
        # self.trajectory_publisher.publish_info()

    def trajectory_1(self):
        print("Executing Trajectory 1")
               
        waypoints = []

        if self.cartesian:
            waypoints.append(self.start_pose)

        wpose = deepcopy(self.start_pose)

        # 1.1 8/0/0/0/0
        wpose.position.x = 0.031240351682225935
        wpose.position.y = 0.11657024706040157
        wpose.position.z = 0.35980622423789693
        wpose.orientation.x = -5.071551442841066e-05
        wpose.orientation.y = 3.5433726337292932e-06
        wpose.orientation.z = -0.06971326667696516
        wpose.orientation.w = 0.9975670693565458
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.2 13/0/0/0/0
        wpose.position.x = 0.0505023471504527
        wpose.position.y = 0.11297343497763537
        wpose.position.z = 0.3598345306461554
        wpose.orientation.x = 1.2196291363768012e-05
        wpose.orientation.y = -1.3892017988867915e-06
        wpose.orientation.z = -0.1132341862000632
        wpose.orientation.w = 0.9935683262488444
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.3 20/0/0/0/0
        wpose.position.x = 0.07677171222786823
        wpose.position.y = 0.10520350821222695
        wpose.position.z = 0.3597997151177298
        wpose.orientation.x = -6.436007182513625e-05
        wpose.orientation.y = 1.1346520069511344e-05
        wpose.orientation.z = -0.17371348644207885
        wpose.orientation.w = 0.9847962329117508
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.4 20/0/0/0/0
        wpose.position.x = 0.09836519341012566
        wpose.position.y = 0.09604870008925734
        wpose.position.z = 0.35980923806960247
        wpose.orientation.x = -4.298644666378746e-05
        wpose.orientation.y = 9.927224492442027e-06
        wpose.orientation.z = -0.22496258610384628
        wpose.orientation.w = 0.9743674013979969
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.5 26/40/-75/-20/0
        wpose.position.x = 0.05450377887113492
        wpose.position.y = 0.0061391458111090536
        wpose.position.z = 0.12431796491734401
        wpose.orientation.x = -0.4499305923425203
        wpose.orientation.y = 0.1038474123636137
        wpose.orientation.z = -0.1995171072180362
        wpose.orientation.w = 0.8642748989453743
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.6 26/20/-75/-4/0
        wpose.position.x = 0.0775675646742458
        wpose.position.y = 0.05345368227938003
        wpose.position.z = 0.10690652370061177
        wpose.orientation.x = -0.47974382672610255
        wpose.orientation.y = 0.11073544784991857
        wpose.orientation.z = -0.19576329531174247
        wpose.orientation.w = 0.8480921256068593

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)
        
        # 1.7 26/10/-75/8/0
        wpose.position.x = 0.0938519077860496
        wpose.position.y = 0.08688879729082807
        wpose.position.z = 0.09849178456661609
        wpose.orientation.x = -0.4649051989989751
        wpose.orientation.y = 0.10731997236673906
        wpose.orientation.z = -0.19762362378299111
        wpose.orientation.w = 0.8563238188896795

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.8 26/25/-100/32/0
        wpose.position.x = 0.10190871411076276
        wpose.position.y = 0.10342301910338081
        wpose.position.z = 0.10543615980302509
        wpose.orientation.x = -0.3570956933331718
        wpose.orientation.y = 0.08244721964637118
        wpose.orientation.z = -0.20921162000152874
        wpose.orientation.w = 0.9065956208982152

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.9 26/40/-124/55/0
        wpose.position.x = 0.10555546016283122
        wpose.position.y = 0.11076689249765213
        wpose.position.z = 0.11497877279670789
        wpose.orientation.x = -0.24396667093875943
        wpose.orientation.y = 0.056309724689993235
        wpose.orientation.z = -0.217829128250576
        wpose.orientation.w = 0.9433238835426517

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

         # 1.10 26/50/-148/91/0
        wpose.position.x = 0.10967026477008365
        wpose.position.y = 0.11923649152795966
        wpose.position.z = 0.15288573197671254
        wpose.orientation.x = -0.059521952960639925
        wpose.orientation.y = 0.013735821129149126
        wpose.orientation.z = -0.22454881690459033
        wpose.orientation.w = 0.9725462935821657

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.11平移 22/50/-148/91/0
        wpose.position.x = 0.093720763189338
        wpose.position.y = 0.12633408249197853
        wpose.position.z = 0.1528481757837365
        wpose.orientation.x = -0.0600125654349569
        wpose.orientation.y = 0.011661802473411979
        wpose.orientation.z = -0.19045838358154457
        wpose.orientation.w = 0.9797898236236644

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.12平移 18/50/-148/91/0
        wpose.position.x = 0.07735415631251287
        wpose.position.y = 0.1322998604888187
        wpose.position.z = 0.15285187095241054
        wpose.orientation.x = -0.06037918897139795
        wpose.orientation.y = 0.009562981786494297
        wpose.orientation.z = -0.15621742509708259
        wpose.orientation.w = 0.9858291023369846

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.13平移 18/50/-145/91/0
        # 1.13平移 18/50/-140/91/0
        wpose.position.x = 0.07802383093662281
        wpose.position.y = 0.1343371952672179
        wpose.position.z = 0.20087937113738238
        wpose.orientation.x = 0.008600854238775522
        wpose.orientation.y = -0.0013629277011399004
        wpose.orientation.z = -0.15650991738688438
        wpose.orientation.w = 0.9876379971902638

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.14平移 12/50/-140/91/0
        wpose.position.x = 0.05244272759307842
        wpose.position.y = 0.1411959175384837
        wpose.position.z = 0.20088287161086368
        wpose.orientation.x = 0.008684672281378305
        wpose.orientation.y = -0.0009127371922042527
        wpose.orientation.z = -0.10442695409749694
        wpose.orientation.w = 0.994494220514178

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.15往后缩 12/60/-150/91/0
        wpose.position.x = 0.04997896055989197
        wpose.position.y = 0.12957106005078503
        wpose.position.z = 0.17982607489067434
        wpose.orientation.x = 6.074824626952852e-06
        wpose.orientation.y = -6.386726918991548e-07
        wpose.orientation.z = -0.10444034571418614
        wpose.orientation.w = 0.9945311529307617

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.16送食 0/-45/0/45/0
        wpose.position.x = 7.049465855556011e-05
        wpose.position.y = 0.28844833329508146
        wpose.position.z = 0.28955458392890854
        wpose.orientation.x = 2.880176438026441e-05
        wpose.orientation.y = -2.323336038735687e-09
        wpose.orientation.z = -7.5493518036726685e-06
        wpose.orientation.w = 0.9999999995567329

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        if self.cartesian:
            fraction = 0.0  
            maxtries = 100   
            attempts = 0    
       
        self.arm.set_start_state_to_current_state()
     
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path (
                                    waypoints,   
                                    0.01,       
                                    0.0,        
                                    True)       
           
            attempts += 1
           
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) + " attempts...")

        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the self.arm.")
            new_traj = self.scale_trajectory_speed(plan, 1)
            self.arm.execute(new_traj)
            rospy.loginfo("Path execution complete.")

        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        rospy.sleep(1)
        pass

    def trajectory_2(self):
        print("Executing Trajectory 2")

        # 移动到 "home" 位置
        self.arm.set_named_target("home")
        self.arm.go(wait=True)  # 执行回到 "home" 位置的操作

        waypoints = []
        # 第1个目标位姿
        pose1 = geometry_msgs.msg.Pose()
        pose1.position.x = -0.04126152569147448
        pose1.position.y = -1.1608271043697919e-05
        pose1.position.z = 0.13466731105138943
        pose1.orientation.x = -2.5329329411066154e-05
        pose1.orientation.y = 0.4617385653384503
        pose1.orientation.z = -2.1911380437507107e-05
        pose1.orientation.w = 0.8870160630774994
        waypoints.append(pose1)

        # 第2个目标位姿 0/-58/115/-3/0
        pose2 = geometry_msgs.msg.Pose()
        pose2.position.x = 0.004651029454941641
        pose2.position.y = -2.046464646042786e-06
        pose2.position.z = 0.07989258904926122
        pose2.orientation.x = -3.9551522751682376e-05
        pose2.orientation.y = 0.4523160885130879
        pose2.orientation.z = 6.4712743132175465e-06
        pose2.orientation.w = 0.8918576985517478
        waypoints.append(pose2)

        # 第3个目标位姿 0/-40/110/-5/0
        pose3 = geometry_msgs.msg.Pose()
        pose3.position.x = 0.01789141509686934
        pose3.position.y = 7.276487934794349e-06
        pose3.position.z = 0.06488137527862559
        pose3.orientation.x = -2.2364126936016326e-05
        pose3.orientation.y = 0.5372978736854883
        pose3.orientation.z = 2.2757634687325326e-05
        pose3.orientation.w = 0.8433925503079744
        waypoints.append(pose3)

        # 4
        pose4 = geometry_msgs.msg.Pose()
        pose4.position.x = 0.051483970715395344
        pose4.position.y = 1.4241112043635774e-05
        pose4.position.z = 0.07104692950841252
        pose4.orientation.x = -5.806514357607559e-05
        pose4.orientation.y = 0.551892282402298
        pose4.orientation.z = 3.384697146741942e-05
        pose4.orientation.w = 0.833915405846183
        waypoints.append(pose4)

        # 5
        pose5 = geometry_msgs.msg.Pose()
        pose5.position.x = 0.09042313675275213
        pose5.position.y = -0.00034568300011208987
        pose5.position.z = 0.09881311445761695
        pose5.orientation.x = -0.002231660204876263
        pose5.orientation.y = 0.4383460794272583
        pose5.orientation.z = -0.0012223319941349234
        pose5.orientation.w = 0.898802670360952
        waypoints.append(pose5)

        # 第6个目标位姿 0/-68/142/-62/0 举平
        pose6 = geometry_msgs.msg.Pose()
        pose6.position.x = 0.12194636235398673
        pose6.position.y = -0.0002343643231762216
        pose6.position.z = 0.14228208314683233
        pose6.orientation.x = -0.0005234515020643942
        pose6.orientation.y = 0.10451258921853264
        pose6.orientation.z = -0.0013561753209920613
        pose6.orientation.w = 0.9945225012446235
        waypoints.append(pose6)

        # 第7个目标位姿 0/-84/154/-70/0 举更平
        pose7 = geometry_msgs.msg.Pose()
        pose7.position.x = 0.11562327754347726
        pose7.position.y = -0.0011457753343090892
        pose7.position.z = 0.15616169872500374
        pose7.orientation.x = 1.8965798195490206e-06
        pose7.orientation.y = -1.891553357667452e-05
        pose7.orientation.z = -0.003245741669215483
        pose7.orientation.w = 0.9999947323859374
        waypoints.append(pose7)

        # 第8个目标位姿 -15/-84/154/-70/0
        pose8 = geometry_msgs.msg.Pose()
        pose8.position.x = 0.10728169840615166
        pose8.position.y = -0.06226582503019565
        pose8.position.z = 0.15615948791782155
        pose8.orientation.x = 2.1775193787244733e-06
        pose8.orientation.y = 2.53609971874923e-06
        pose8.orientation.z = -0.13368801580547648
        pose8.orientation.w = 0.99102346814736
        waypoints.append(pose8)

        # 第9个目标位姿 -15/-58/140/-82/0
        pose9 = geometry_msgs.msg.Pose()
        pose9.position.x = 0.1420137748854844
        pose9.position.y = -0.07044485406780186
        pose9.position.z = 0.19648551082053417
        pose9.orientation.x = -1.1484424425122635e-05
        pose9.orientation.y = -0.00010129131583014448
        pose9.orientation.z = -0.1306032067157323
        pose9.orientation.w = 0.9914347139392211
        waypoints.append(pose9)

        # 第10个目标位姿 0/-58/140/-82/0
        pose10 = geometry_msgs.msg.Pose()
        pose10.position.x = 0.15129617401189063
        pose10.position.y = -8.239664264766373e-06
        pose10.position.z = 0.19647524674397032
        pose10.orientation.x = 1.8308086152070256e-06
        pose10.orientation.y = -6.915397243407718e-05
        pose10.orientation.z = -5.472007928719286e-07
        pose10.orientation.w = 0.9999999976070384
        waypoints.append(pose10)

        # 第11个目标位姿 
        pose11 = geometry_msgs.msg.Pose()
        pose11.position.x = 0.4390590994135001
        pose11.position.y = 2.251248707737093e-05
        pose11.position.z = 0.2563480481437091
        pose11.orientation.x = 1.833107918216095e-06
        pose11.orientation.y = -5.3289826445893536e-05
        pose11.orientation.z = 6.47960435497286e-05
        pose11.orientation.w = 0.9999999964791535
        waypoints.append(pose11)

        # 规划并执行笛卡尔轨迹
        (plan, fraction) = self.arm.compute_cartesian_path(
            waypoints,   # 目标位姿序列
            0.01,        # eef_step，终端执行器的步长
            False)         # jump_threshold，跳跃阈值
        print("Fraction of path achieved: ", fraction)

        # 执行轨迹
        self.arm.execute(plan, wait=True)

        # 停止运动
        self.arm.stop()

    def trajectory_3(self):
        print("Executing Trajectory 3")
        # 初始化路点列表
        waypoints = []

        # 将初始位姿加入路点列表
        if self.cartesian:
            waypoints.append(self.start_pose)

        # 设置路点数据，并加入路点列表
        wpose = deepcopy(self.start_pose)
        # 3.1 -8/0/0/0/0
        wpose.position.x = -0.031168722768712603
        wpose.position.y = 0.11657380103657414
        wpose.position.z = 0.3598317930311014
        wpose.orientation.x = 6.1553480981672e-06
        wpose.orientation.y = 4.3050536038942754e-07
        wpose.orientation.z = 0.06977623706470132
        wpose.orientation.w = 0.9975626680580107
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.2 -13/0/0/0/0
        wpose.position.x = -0.050409755512666105
        wpose.position.y = 0.11298088749099783
        wpose.position.z = 0.3598469067566341
        wpose.orientation.x = 3.9616285959787196e-05
        wpose.orientation.y = 4.51585058841313e-06
        wpose.orientation.z = 0.11324750712240661
        wpose.orientation.w = 0.9935668072861118

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)
        
        # 3.3 -20/0/0/0/0
        wpose.position.x = -0.07664706569283344
        wpose.position.y = 0.10523798361178978
        wpose.position.z = 0.3598212338774806
        wpose.orientation.x = -1.7106647426589924e-05
        wpose.orientation.y = -3.0177083734344027e-06
        wpose.orientation.z = 0.17363835161486693
        wpose.orientation.w = 0.9848094854065572

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.4 -26/0/-0/0/0
        wpose.position.x = -0.09823540746879462
        wpose.position.y = 0.0960638282344723
        wpose.position.z = 0.35983608304534126
        wpose.orientation.x = 1.533401091891952e-05
        wpose.orientation.y = 3.540172588279489e-06
        wpose.orientation.z = 0.22491285700790822
        wpose.orientation.w = 0.9743788824193983

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.5 -26/40/-75/-20/0
        wpose.position.x = -0.05442712271251495
        wpose.position.y = 0.006189099692509581
        wpose.position.z = 0.12431178334557616
        wpose.orientation.x = -0.44996849402342126
        wpose.orientation.y = -0.10379707122178664
        wpose.orientation.z = 0.19951514382580832
        wpose.orientation.w = 0.8642616674226856

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

         # 3.6 -26/20/-75/-4/0
        wpose.position.x = -0.07746814540877038
        wpose.position.y = 0.053478234306858746
        wpose.position.z = 0.10686112144599383
        wpose.orientation.x = -0.47984713180284405
        wpose.orientation.y = -0.11075659848244414
        wpose.orientation.z = 0.19574938248383222
        wpose.orientation.w = 0.8480341297674175

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)


        # 3.7 -26/10/-75/8/0
        wpose.position.x = -0.09378779016725308
        wpose.position.y = 0.08690792190055481
        wpose.position.z = 0.09847789076652683
        wpose.orientation.x = -0.4649318272946334
        wpose.orientation.y = -0.10731542198475902
        wpose.orientation.z = 0.1976925996181981
        wpose.orientation.w = 0.8562940103894837

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.8 -26/25/-100/32/0
        wpose.position.x = -0.10185816545490319
        wpose.position.y = 0.10347783908022863
        wpose.position.z = 0.10546227953481535
        wpose.orientation.x = -0.3570241013462167
        wpose.orientation.y = -0.0824241581443727
        wpose.orientation.z = 0.2092844039348642
        wpose.orientation.w = 0.9066091150444863

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.9  轨迹点2.2 -26/40/-124/55/0
        wpose.position.x = -0.1054117011712569
        wpose.position.y = 0.1108467509634318
        wpose.position.z = 0.11499687384853347
        wpose.orientation.x = -0.2439275718634523
        wpose.orientation.y = -0.056312765155181785
        wpose.orientation.z = 0.21768761051587665
        wpose.orientation.w = 0.9433664804270214

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

         # 3.10 轨迹点2.3 -26/50/-148/91/0
        wpose.position.x = -0.10954038258269447
        wpose.position.y = 0.1192944483298219
        wpose.position.z = 0.1528943970034999
        wpose.orientation.x = -0.05946987362727919
        wpose.orientation.y = -0.013733472824132257
        wpose.orientation.z = 0.22443885251374499
        wpose.orientation.w = 0.9725748954899349

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.11平移 -22/50/-148/91/0
        wpose.position.x = -0.0936176385542106
        wpose.position.y = 0.12637390058752562
        wpose.position.z = 0.1529021353704747
        wpose.orientation.x = -0.05990584566501952
        wpose.orientation.y = -0.011646877158218976
        wpose.orientation.z = 0.19042512117381682
        wpose.orientation.w = 0.9798029971037838

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.12平移 -18/50/-148/91/0
        wpose.position.x = -0.07720428524418477
        wpose.position.y = 0.13233934404644604
        wpose.position.z = 0.1529294124429962
        wpose.orientation.x = -0.0602170034778715
        wpose.orientation.y = -0.009538775824991285
        wpose.orientation.z = 0.1560948211728744
        wpose.orientation.w = 0.9858586770176114

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.13平移 18/50/-145/91/0
        # 3.13平移 -18/50/-140/91/0
        wpose.position.x = -0.07786707453417932
        wpose.position.y = 0.13437065705025183
        wpose.position.z = 0.20096614055894246
        wpose.orientation.x = 0.008781171200801458
        wpose.orientation.y = 0.0013903418009307096
        wpose.orientation.z = 0.15639470372149342
        wpose.orientation.w = 0.9876546231501603

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.14平移 -12/50/-140/91/0
        wpose.position.x = -0.05239355358617451
        wpose.position.y = 0.14118167702093876
        wpose.position.z = 0.2009817571139045
        wpose.orientation.x = 0.00889233848707391
        wpose.orientation.y = 0.0009341414512527173
        wpose.orientation.z = 0.10455471516283729
        wpose.orientation.w = 0.9944789415735249

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.15往后缩 -12/60/-150/91/0
        wpose.position.x = -0.04989556381254159
        wpose.position.y = 0.1294807134301522
        wpose.position.z = 0.1837350610560631
        wpose.orientation.x = 0.008634658197445457
        wpose.orientation.y = 0.0009074450860638917
        wpose.orientation.z = 0.10452561069753129
        wpose.orientation.w = 0.9944842964720646

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.16送食 0/-45/0/45/0
        wpose.position.x = 7.049465855556011e-05
        wpose.position.y = 0.28844833329508146
        wpose.position.z = 0.28955458392890854
        wpose.orientation.x = 2.880176438026441e-05
        wpose.orientation.y = -2.323336038735687e-09
        wpose.orientation.z = -7.5493518036726685e-06
        wpose.orientation.w = 0.9999999995567329

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        if self.cartesian:
            fraction = 0.0   
            maxtries = 100   
            attempts = 0     
       
        self.arm.set_start_state_to_current_state()
     
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path (
                                    waypoints, 
                                    0.01,       
                                    0.0,         
                                    True)      
           
            attempts += 1
           
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                     
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the self.arm.")
            new_traj = self.scale_trajectory_speed(plan, 1)
            self.arm.execute(new_traj)
            rospy.loginfo("Path execution complete.")
            
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        rospy.sleep(1)
        pass

    def scale_trajectory_speed(self, traj, scale):
       
       # Create a new trajectory object
       new_traj = RobotTrajectory()
       
       # Initialize the new trajectory to be the same as the input trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
 
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
       
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
        
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
       
       # Cycle through all points and joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj

    def go_home(self):
        print("Returning to home position")
        self.arm.set_named_target('home')
        self.arm.go()
        rospy.sleep(1)

    def shutdown(self):
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    print("Press '1', '2', or '3' to execute  trajectories, or '0' to return to home position")
    demo = MoveItCartesianDemo()  # 创建实例
    while True:
        try:
            key = getKey()
            if key == '1':
                demo.execute_trajectory(demo.trajectory_1,1)
            elif key == '2':
                demo.execute_trajectory(demo.trajectory_2,2)
            elif key == '3':
                demo.execute_trajectory(demo.trajectory_3,3)
            elif key == '0':
                demo.go_home()
            elif key == 'q':
                print("Exiting...")
                break
        except Exception as e:
            print("Error:", e)
            break

    demo.shutdown()  # 正确关闭moveit